#include "types.h"
#include "target.h"
#include "adl_system_control_block.h"

#include "../config/hal_boot_config.h"
#include "hal_boot.h"

static U32 hal_boot_getSystemFrequency(void)
{
	U32  msel;
    U32  nsel;
    U32  fin;
    U32  pll_clk_feq;                                    /* When the PLL is enabled, this is Fcco                    */
    U32  clk_div;
    U32  clk_freq;


    switch (adl_scb_GET_PLL_CLOCK_SOURCE()) {           /* Determine the current clock source                       */
        case 0:
             fin = INTERNAL_RC_OSC_FRQ;
             break;

        case 1:
             fin = MAIN_OSC_FRQ;
             break;

        case 2:
             fin = RTC_OSC_FRQ;
             break;

        default:
             fin = INTERNAL_RC_OSC_FRQ;
             break;
    }

    if (adl_scb_IS_PLL_CONNECTED()) {                     /* If the PLL is currently enabled and connected        */
        msel        = adl_scb_GET_ACTUAL_PLL_MUL() + 1;   /* Obtain the PLL multiplier                            */
        nsel        = adl_scb_GET_ACTUAL_PLL_DIV() + 1;   /* Obtain the PLL divider                               */
        pll_clk_feq = (2 * msel * (fin / nsel));          /* Compute the PLL output frequency                     */
    } else {
        pll_clk_feq = (fin);                              /* The PLL is bypassed                                  */
    }

    clk_div         = adl_scb_GET_CPU_CLOCK_DIVISOR() + 1;   /* Obtain the CPU core clock divider                    */
    clk_freq        = (U32)(pll_clk_feq / clk_div);          /* Compute the ARM Core clock frequency                 */

    return (clk_freq);
}

static void hal_boot_configurePLL(void)
{
  	if (adl_scb_IS_PLL_CONNECTED())
  	{
		// Enable PLL, disconnected
		adl_scb_ENABLE_PLL();
  	}

  	// Disable PLL, disconnected
  	adl_scb_DISABLE_PLL();

  	// Enable main OSC
  	adl_scb_ENABLE_MAIN_OSCILATOR();
  	// Wait until main OSC is usable
  	while(!adl_scb_IS_MAIN_OSCILATOR_READY());

	// Select main OSC, as the PLL clock source
  	adl_scb_SET_PLL_CLOCK_SOURCE(PLL_SOURCE_MAIN_OSC);

  	adl_scb_SET_PLL_CONFIGURATION(PLL_MValue, PLL_NValue);

  	// Enable PLL, disconnected
	adl_scb_ENABLE_PLL();

  	adl_scb_SET_CPU_CLOCK_DIVISOR(CCLKCFG_Val);
  	adl_scb_SET_USB_CLOCK_DIVISOR(USBCLKCFG_Val);

  	// Check lock bit status
  	while (!adl_scb_IS_PLL_LOCKED());

  	// Wait until reach target PLL clock
  	while ((adl_scb_GET_ACTUAL_PLL_MUL() != PLL_MValue)
  			&& ( adl_scb_GET_ACTUAL_PLL_DIV() != PLL_NValue));

	adl_scb_CONNECT_PLL();

  	// Check connect bit status
	while (!adl_scb_IS_PLL_CONNECTED());
}

static void hal_boot_configureMAM(void)
{
	U32  systemFreq;

    systemFreq = hal_boot_getSystemFrequency();			        /* Get the current core clock frequency                     */

    MAMCR    = 0;                                               /* Disable MAM functionality                                */

    if (systemFreq < 20000000) {                                /* Compare current clock frequency with MAM modes           */
        MAMTIM = 1;                                             /* Set MAM fetch cycles to 1 processor clock in duration    */
    }

    if (systemFreq < 40000000) {
        MAMTIM = 2;                                             /* Set MAM fetch cycles to 2 processor clock in duration    */
    }

    if (systemFreq >= 40000000) {
        MAMTIM = 3;                                             /* Set MAM fetch cycles to 3 processor clock in duration    */
    }

    MAMCR = 2;                                                  /* Enable full MAM functionality                            */
}

void hal_boot_start(void)
{
	hal_boot_configurePLL();
	hal_boot_configureMAM();
}
